﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using Microsoft.Kinect;
using System.Speech;
using Microsoft.Speech;
using Microsoft.Speech.Recognition;
using Microsoft.Speech.AudioFormat;
using Microsoft.Samples.Kinect.WpfViewers;
using System.Windows;

namespace KinBot_Controller_V2
{
    

   public class kinect
    {
        KinectSensor newSensor;

        // scene they are considered a new skeleton
        const int skeletonCount = 6;

        // the array that will hold the skeletons.
        Skeleton[] allSkeletons = new Skeleton[skeletonCount];

        //build speech engine
        private SpeechRecognitionEngine speechEngine;
        // instanciates the speech recognizer engine
        private SpeechRecognitionEngine speechRecognizer;


        byte[] data = new byte[4];

        //chest depth data
        double centerData;
        //left hand depth
        double calibX;
        //right hand depth
        double calibY;
        //data being worked on and trucated for each had
        double truncX;
        double truncY;
        //data that is going to be sent to the App.fez
        double convertX;
        double convertY;
        //
        double combineXY;
        //used to calculate angle of arm
        public double offSetA = 0;
        // the starting positon for the camera angle in degrees
        int camPos = 170;
        // used to limit the amount of data that is sent to the App.fez, one in every 10
        int sendCounterX = 0;
        int sendCounterY = 1;

        //on off states for each part of the robot. Turned on and off by voice
        bool startData = false;
        bool carEnabled = false;
        bool handEnabled = false;
        bool allStopEnabled = false;
        //lets the dispose method know if the program is closing, also used to shut down the kinect
        bool closing = false;
        //set and intialize the conter for sendTo method
        bool sendToCount = false;



        public bool camInitiated = false;

        public bool skeletonInt = false;


        public string lightUpState = "";

        public Skeleton first;

        public AllFramesReadyEventArgs framePass;

        // sets the amount of skeletons that will be held in memory, each time a new person comes inthe

        public bool setKinect(object sender, DependencyPropertyChangedEventArgs e)
        {
            KinectSensor oldSensor = (KinectSensor)e.OldValue;

            stopKinect(oldSensor);

            newSensor = (KinectSensor)e.NewValue;

            if (newSensor == null)
            {
                return false;
            }

            newSensor.SkeletonStream.Enable();
            newSensor.DepthStream.Enable();
            newSensor.ColorStream.Enable();





            newSensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(newSensor_AllFramesReady);

            try
            {
                newSensor.Start();
                return true;
            }
            catch (System.IO.IOException)
            {
                return false;
                //kinectSensorChooser1.AppConflictOccurred();
            }
        }

        void  newSensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {

            if (closing)
            {
                return;
            }

            //Get a skeleton
            first = GetFirstSkeleton(e);



            if (first == null)
            {
                return;
            }


            // ScalePosition(leftEllipse, first.Joints[JointType.HandLeft]);
            // ScalePosition(rightEllipse1, first.Joints[JointType.HandRight]);

            //GetCameraPoint(first, e);
            skeletonInt = true;
            
            framePass = e;
        }

        public bool isReady()
        {
            return skeletonInt;
        }



        // creates skeleton information from the raw data

        Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e)
        {
            using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
            {

                // stops the motors when no skeleton is detected
                if (skeletonFrameData == null)
                {
                    //App.fez.sendData("X0");
                    //App.fez.sendData("Y0");

                    return null;
                }

                //copys skeleton data to the allSkeletons array of skeletons
                skeletonFrameData.CopySkeletonDataTo(allSkeletons);

                //get the first tracked skeleton
                Skeleton first = (from s in allSkeletons
                                  where s.TrackingState == SkeletonTrackingState.Tracked
                                  select s).FirstOrDefault();

                return first;

            }
        }

        public string GetCameraPoint(Skeleton first, AllFramesReadyEventArgs e)
        {

            using (DepthImageFrame depth = e.OpenDepthImageFrame())
            {


                //Map a joint location to a point on the depth map
                //sholder center
                DepthImagePoint sholderDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.ShoulderCenter].Position);

                centerData = sholderDepthPoint.Depth;


                //left hand
                DepthImagePoint leftDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.HandLeft].Position);

                calibX = leftDepthPoint.Depth;

                //Right Hand
                DepthImagePoint rightDepthPoint =
                    depth.MapFromSkeletonPoint(first.Joints[JointType.HandRight].Position);

                calibY = rightDepthPoint.Depth;

                // Use the chest to calculate the distances of the arms. This will allow me to not have a calibration so it works from any distance within sensor range.
                convertX = (centerData - calibX) / 3;
                convertY = (centerData - calibY) / 3;
                combineXY = (convertX + convertY) / 2;

                //truncated = BitConverter.GetBytes((Math.Truncate(convertX)));              
                truncX = (Math.Truncate(convertX));
                truncY = (Math.Truncate(convertY));

                // alert.Text = truncX.ToString();

                //combined.Text = (Math.Truncate(combineXY).ToString());

                //used to find the angle between the right wrist and the right sholder. 
                //  double xe = Math.Truncate(jointAngleCalc(first));



                ColorImagePoint sholderColorPoint =
                    depth.MapToColorImagePoint(sholderDepthPoint.X, sholderDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                //left hand
                ColorImagePoint leftColorPoint =
                    depth.MapToColorImagePoint(leftDepthPoint.X, leftDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                //right hand
                ColorImagePoint rightColorPoint =
                    depth.MapToColorImagePoint(rightDepthPoint.X, rightDepthPoint.Y,
                    ColorImageFormat.RgbResolution640x480Fps30);
                

                //*** Testing new switch statement and sendTo method to attempt to speed up reponse time ****
                switch (sendTo())
                {
                    case "X":



                        //alert.Text = truncX.ToString();

                        //leftSlider.Value = truncX;

                        string returningX = "X"+truncX;


                        if (startData == true)
                        {

                            App.fez.sendData(App.move.convertSend_X(truncX));
                            return returningX;
                        }

                        //lftVal.Text = Math.Truncate(convertX).ToString();
                        return returningX;

                        

                    case "Y":

                        //rtVal.Text = move.convertSend_Y(truncY);
                        //rtSlider.Value = truncY;

                        string returningY = "Y"+truncY;

                        if (startData == true)
                        {
                            App.fez.sendData(App.move.convertSend_Y(truncY));
                            return returningY;
                        }
                        return returningY;
                      

                    case "N":

                        return "";

                }

                return "";


            }
        }

        private string sendTo()
        {
            if (sendCounterX == 1)
            {
                sendToCount = false;
                sendCounterX = 0;
                return "X";
            }
            else if (sendCounterY == 2)
            {
                sendToCount = true;
                sendCounterY = 1;
                return "Y";
            }

            else
            {
                sendCounterX++;
                sendCounterY++;
                return "N";
            }
        }

        private void stopKinect(KinectSensor sensor)
        {
            if (sensor != null)
            {
                if (sensor.IsRunning)
                {
                    //stop sensor 
                    sensor.Stop();

                    //stop audio if not null
                    if (sensor.AudioSource != null)
                    {
                        sensor.AudioSource.Stop();
                    }
                }
            }
        }



        public void setAudioPrefs()
        {
            var audioSource = newSensor.AudioSource;

            audioSource.BeamAngleMode = BeamAngleMode.Adaptive;

            //added to help remove background noise. remove if audio is not working.
            audioSource.NoiseSuppression = true;

            var kinectStream = audioSource.Start();

            speechRecognizer.SetInputToAudioStream(kinectStream, new SpeechAudioFormatInfo(EncodingFormat.Pcm, 16000, 16, 1, 32000, 2, null));

            speechRecognizer.RecognizeAsync(RecognizeMode.Multiple);

            newSensor.AudioSource.EchoCancellationMode = EchoCancellationMode.None;

            newSensor.AudioSource.AutomaticGainControlEnabled = false;
        }


        public RecognizerInfo GetKinectRecognizer()
        {
            foreach (RecognizerInfo recognizer in SpeechRecognitionEngine.InstalledRecognizers())
            {
                string value;
                recognizer.AdditionalInfo.TryGetValue("Kinect", out value);
                if ("True".Equals(value, StringComparison.OrdinalIgnoreCase) && "en-US".Equals(recognizer.Culture.Name, StringComparison.OrdinalIgnoreCase))
                {
                    return recognizer;
                }
            }

            return null;
        }

        public SpeechRecognitionEngine createSpeechRecognizer()
        {

            RecognizerInfo ri = GetKinectRecognizer();



            this.speechEngine = new SpeechRecognitionEngine(ri.Id);


            //programmicaly creates a grammer file
            //What you can say
            var directions = new Choices();
            directions.Add("start");
            directions.Add("stop");
            directions.Add("car");
            directions.Add("Hand");
            directions.Add("Camera Up");
            directions.Add("Camera Down");
            directions.Add("Forward");
            directions.Add("Backward");
            directions.Add("Turn Left");
            directions.Add("Turn Right");


            var gb = new GrammarBuilder { Culture = ri.Culture };
            gb.Append(directions);

            var g = new Grammar(gb);
            speechEngine.LoadGrammar(g);


            speechEngine.SpeechRecognized += speechEngine_SpeechRecognized;
            speechEngine.SpeechRecognitionRejected += speechEngine_SpeechRecognitionRejected;
            speechEngine.SpeechHypothesized += speechEngine_SpeechHypothesized;

            return speechEngine;

            //speechEngine.SetInputToAudioStream(newSensor.AudioSource.Start(), new SpeechAudioFormatInfo(EncodingFormat.Pcm, 16000, 16, 1, 32000, 2, null));
            //speechEngine.RecognizeAsync(RecognizeMode.Multiple);

        }

        private void speechEngine_SpeechRecognitionRejected(object sender, SpeechRecognitionRejectedEventArgs e)
        {
            //alert.Text = "Not recognized, please try again.";


        }

        private void speechEngine_SpeechHypothesized(object sender, SpeechHypothesizedEventArgs e)
        {

            //alert.Text = e.Result.Text;

            //alert.Text = "Hypothosized: " + e.Result.Text + " " + e.Result.Confidence;

        }

        private void speechEngine_SpeechRecognized(object sender, SpeechRecognizedEventArgs e)
        {
           lightUpState = e.Result.Text.ToLowerInvariant();


            switch (lightUpState)
            {

                case "stop":

                    if (allStopEnabled == false)
                    {
                        App.fez.sendData("X0");
                        App.fez.sendData("Y0");


                        startData = false;
                        allStopEnabled = true;
                        handEnabled = false;
                        carEnabled = false;

                        //power.Source = imgChanger.setImages("stop");





                    }
                    break;

                //case "hand":

                //    if (handEnabled == false)
                //    {
                //        handEnabled = true;
                //        allStopEnabled = false;
                //        carEnabled = false;

                //        //colorBrush.Color = System.Windows.Media.Color.FromRgb(0, 0, 248);
                //        //blue.Fill = colorBrush;

                //        //red.Fill = colorBrush2;
                //        //green.Fill = colorBrush2;

                //    }

                //    break;

                case "car":

                    if (carEnabled == false)
                    {
                        carEnabled = true;
                        allStopEnabled = false;
                        handEnabled = false;

                        //colorBrush.Color = System.Windows.Media.Color.FromRgb(0, 250, 0);
                        //green.Fill = colorBrush;

                        //red.Fill = colorBrush2;
                        //blue.Fill = colorBrush2;

                    }
                    else
                    {

                    }

                    break;

                case "start":

                    startData = true;
                    allStopEnabled = false;

                    //power.Source = imgChanger.setImages("start");

                    break;


                case "camera up":
                    {
                        //cameraLens.Source = imgChanger.setImages("cameraOn");
                        //cameraUp.Source = imgChanger.setImages("cameraUp");

                        //cameraDown.Source = imgChanger.setImages("cameraDownOff");
                        
                        camPos -= 10;
                        App.fez.sendData("Z" + camPos);
                        //cameraUp.Source = imgChanger.setImages("cameraUpOff");
                        //cameraLens.Source = imgChanger.setImages("cameraOff");
                        break;
                    }
                case "camera down":
                    {
                        //cameraLens.Source = imgChanger.setImages("cameraOn");
                        //cameraDown.Source = imgChanger.setImages("cameraDown");
                        //cameraUp.Source = imgChanger.setImages("cameraUpOff");
                        camInitiated = false;
                        camPos += 10;
                        App.fez.sendData("Z" + camPos);
                        //cameraLens.Source = imgChanger.setImages("cameraOff");
                        //cameraDown.Source = imgChanger.setImages("cameraDownOff");


                        //cameraDown.Source = imgChanger.setImages("cameraDownOff");
                        break;
                    }
            }


        }

    }
}
